#include "ADIS16405.h"
#include "board_config.h"

const uint8_t dummy_tx[ADIS16405_BURST_READ_LEN] = {
	0xff, 0xff, 0xff, 0xff, 0xff, 0xff,
	0xff, 0xff, 0xff, 0xff, 0xff, 0xff,
	0xff, 0xff, 0xff, 0xff, 0xff, 0xff,
	0xff, 0xff, 0xff, 0xff, 0xff, 0xff
};

void adis16405_start_burst_reading(SPI_HandleTypeDef *spihandler,
				   uint8_t *pRxData)
{
	uint8_t tx_buf[] = {0x3E, 0x00};

	HAL_SPI_Transmit(spihandler, tx_buf, 2, 1000);
	HAL_SPI_TransmitReceive_DMA(spihandler,
				    dummy_tx,
				    pRxData,
				    ADIS16405_BURST_READ_LEN);
}

void adis16405_byte2raw(struct adis16405_raw* raw, uint8_t* byte)
{
	uint8_t i;
	for (i = 0; i < 3; i++) {
		raw->gyr[i] = (byte[i * 2 + 2] & 0x003f) << 8
			| (byte[i * 2 + 3]);
		if(raw->gyr[i]>8192)
			raw->gyr[i] -= 2*8192;
	}
	for (i = 0; i < 3; i++) {
		raw->acc[i] = (byte[i * 2 + 8] & 0x003f) << 8
			| (byte[i * 2 + 9]);
		if(raw->acc[i]>8192)
			raw->acc[i] -= 2*8192;
	}
	for (i = 0; i < 3; i++) {
		raw->mag[i] = (byte[i * 2 + 14] & 0x003f) << 8
			| (byte[i * 2 + 15]);
		if(raw->mag[i]>8192)
			raw->mag[i] -= 2*8192;
	}
}
